#include "imu.h"
static uint16_t times_i = 0;//读取100次磁力计初始数据
static int16_t sum = 0;
static float sample_yaw = 0;//计算初始yaw的采样值
float	kalman_yaw = 0,init_yaw = 0;//全局变量，卡尔曼估计的真实角度、初始yaw角
FLOAT_ANGLE Att_Angle,Att_Angle1,Gyr;          //飞机姿态数据
FLOAT_XYZ 	Gyr_rad,Acc_rad,Gyr_rad_filt,Acc_rad_filt;	              //转换物理单位后的陀螺仪,加速度计数据

float acc_rol,acc_pit,acc1_rol,acc1_pit, gyr_rol, gyr_pit,gyr_yaw,Gyr_yaw,angle_f=0, angle_dot_f=0;
float a_z,a_Z,v_z,a_x,a_X,v_x,a_y,a_Y,v_y,a_x_1,a_y_1;
Butter_BufferData Acc_Filter_Buf[3],Acc_Filter_Buf1[3];
float aax,aay,aaz,aaax,aaay,aaaz;

/*********************************************************************************************************
*函  数：void IMU(void)(void)
*功　能：姿态解算
*参  数：无
*返回值：无
*备  注：此函数对原始数据进行校准（去零偏，滤波处理，赋予物理意义 并解算姿态
**********************************************************************************************************/	
void IMU(void)
{

	BMI088_Read();    //读取IMU数据 加速度、陀螺仪
	HMC58831L_Read(); //磁力计
	
	if (times_i<100)	//前0.5s先估计pit rol
	{
		times_i++;
		
		//加速度AD值 转换成 米/平方秒 
		Acc_rad.X = (float)BMI088_Acc.X * Acc_Gain * acc_scale.X- acc_off.X ;
		Acc_rad.Y = (float)BMI088_Acc.Y * Acc_Gain * acc_scale.Y- acc_off.Y ;  
		Acc_rad.Z = (float)BMI088_Acc.Z * Acc_Gain * acc_scale.Z- acc_off.Z ; 
		
		//陀螺仪AD值 转换成 弧度/秒    
		Gyr_rad.X = (float) BMI088_Gyr.X * Gyro_Gr - gyr_off.X ;  
		Gyr_rad.Y = (float) BMI088_Gyr.Y * Gyro_Gr - gyr_off.Y ;
		Gyr_rad.Z = (float) BMI088_Gyr.Z * Gyro_Gr - gyr_off.Z ;
		
		//加速度低通滤波
		Acc_rad_filt.X=LPButterworth(Acc_rad.X,&Acc_Filter_Buf[0],&Butter_2HZ_Parameter_Acce);
		Acc_rad_filt.Y=LPButterworth(Acc_rad.Y,&Acc_Filter_Buf[1],&Butter_2HZ_Parameter_Acce);
		Acc_rad_filt.Z=LPButterworth(Acc_rad.Z,&Acc_Filter_Buf[2],&Butter_2HZ_Parameter_Acce);
		
		//姿态角原始数据
		Att_Angle1.pit+=Gyr_rad.X*0.5729578f;
		Att_Angle1.rol+=Gyr_rad.Y*0.5729578f;
		Att_Angle1.yaw+=Gyr_rad.Z*0.5729578f;

		//mag_yaw_diff = 0.99f*mag_yaw_diff + 0.01f*(mag_yaw - init_mag_yaw);
		AHRS_compute(Gyr_rad.X, Gyr_rad.Y, Gyr_rad.Z,Acc_rad.X, Acc_rad.Y, Acc_rad.Z); //梯度下降算法
	}
	else if(100<=times_i&&times_i<300)	//采样估计初始yaw角
	{
		kalman_filter_yaw(Gyr_rad.Z*0.5729578f, mag_yaw, &sample_yaw);		//卡尔曼滤波
		if(sample_yaw>180) sample_yaw-=360;
		if(sample_yaw<-180) sample_yaw+=360;
		sum=sum+((int)sample_yaw);
		times_i++;
	}
	else
	{

		init_yaw = ((float)sum)/200;

		//加速度AD值 转换成 米/平方秒 
		Acc_rad.X = (float)BMI088_Acc.X * Acc_Gain * acc_scale.X- acc_off.X ;
		Acc_rad.Y = (float)BMI088_Acc.Y * Acc_Gain * acc_scale.Y- acc_off.Y ;  
		Acc_rad.Z = (float)BMI088_Acc.Z * Acc_Gain * acc_scale.Z- acc_off.Z ; 
		
		//陀螺仪AD值 转换成 弧度/秒    
		Gyr_rad.X = (float) BMI088_Gyr.X * Gyro_Gr - gyr_off.X ;  
		Gyr_rad.Y = (float) BMI088_Gyr.Y * Gyro_Gr - gyr_off.Y ;
		Gyr_rad.Z = (float) BMI088_Gyr.Z * Gyro_Gr - gyr_off.Z ;
		
		//加速度低通滤波
		Acc_rad_filt.X=LPButterworth(Acc_rad.X,&Acc_Filter_Buf[0],&Butter_2HZ_Parameter_Acce);
		Acc_rad_filt.Y=LPButterworth(Acc_rad.Y,&Acc_Filter_Buf[1],&Butter_2HZ_Parameter_Acce);
		Acc_rad_filt.Z=LPButterworth(Acc_rad.Z,&Acc_Filter_Buf[2],&Butter_2HZ_Parameter_Acce);
		
		//姿态角原始数据
		Att_Angle1.pit+=Gyr_rad.X*0.5729578f;
		Att_Angle1.rol+=Gyr_rad.Y*0.5729578f;
		Att_Angle1.yaw+=Gyr_rad.Z*0.5729578f;

		//mag_yaw_diff = 0.99f*mag_yaw_diff + 0.01f*(mag_yaw - init_mag_yaw);
		AHRS_compute(Gyr_rad.X, Gyr_rad.Y, Gyr_rad.Z,Acc_rad.X, Acc_rad.Y, Acc_rad.Z); //梯度下降算法
	}		

	
}


/*********************************************************************************************************
*函  数：void AHRS_compute(float gx, float gy, float gz, float ax, float ay, float az) 
*功　能：梯度下降姿态解算
*参  数：无
*返回值：无
*备  注：无
**********************************************************************************************************/	
float Acceleration_Length=0.0f,hisGYRO_Length=0.0f,GYRO_Length=0.0f,Tmep_gyro_Length[350],Tmep_Acce_Length=0,Quad_Buf[10][4]={0},att_q[4]={1,0,0,0}; 
float GYRO_rms,BETADEF,Beta_Adjust[2]={0.035f,0.025f};
float CNTLCYCLE1=0.005f,CNTLCYCLE2=0.000005f,CNTLCYCLE_X=0.0f,CNTLCYCLE_Y=0.0f,CNTLCYCLE_Z=0.0f;
u16 Quad_Delay=0,numGYRO_Length=0;
float delta;               				//角速度模长 
float BETADEF=0.04f;
float Begain_Imu_Time,End_Imu_time,CNTLCYCLE=0.01f;//周期

void AHRS_compute(float gx, float gy, float gz, float ax, float ay, float az) 
{
  float recipNorm;					           //平方根分之一
	float s0, s1, s2, s3;					      //梯度下降算子求出来的姿态四元数微分
	float qDot1, qDot2, qDot3, qDot4;		//四元数姿态微分
	float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;//四元数乘积组合                                      
	u16 i;
	
//	End_Imu_time=Begain_Imu_Time;//结束时间
//	Begain_Imu_Time=(10000*ms10+TIM2->CNT)/1000; //开始时间 ms
//	CNTLCYCLE=(Begain_Imu_Time-End_Imu_time)/1000;//姿态解算周期 ms化为s

										/////////////陀螺仪数据处理////////////
	GYRO_Length=1/invSqrt(gx*RadtoDeg*gx*RadtoDeg+gy*RadtoDeg*gy*RadtoDeg);      //角速度角度制的模长 
	//角速度数据更新四元数姿态微分
	qDot1 = 0.5f * (-Quad_Buf[Quad_Delay][1] * gx - Quad_Buf[Quad_Delay][2] * gy - Quad_Buf[Quad_Delay][3] * gz); 
	qDot2 = 0.5f * ( Quad_Buf[Quad_Delay][0] * gx + Quad_Buf[Quad_Delay][2] * gz - Quad_Buf[Quad_Delay][3] * gy);
	qDot3 = 0.5f * ( Quad_Buf[Quad_Delay][0] * gy - Quad_Buf[Quad_Delay][1] * gz + Quad_Buf[Quad_Delay][3] * gx);
	qDot4 = 0.5f * ( Quad_Buf[Quad_Delay][0] * gz + Quad_Buf[Quad_Delay][1] * gy - Quad_Buf[Quad_Delay][2] * gx);


										////////////加速度计数据处理//////////
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))      //判断加速度计数据是否有效                                                
	{         
		recipNorm=invSqrt(ax * ax + ay * ay + az * az);     	//数据归一化
		Acceleration_Length=fabs(1/recipNorm-G);                 //加速度模长
		ax *= recipNorm;				//加速度计数据归一化
		ay *= recipNorm;
		az *= recipNorm;

		_2q0 = 2.0f * att_q[0];        //定义过程变量减少计算量
		_2q1 = 2.0f * att_q[1];
		_2q2 = 2.0f * att_q[2]; 
		_2q3 = 2.0f * att_q[3];
		_4q0 = 4.0f * att_q[0];
		_4q1 = 4.0f * att_q[1];
		_4q2 = 4.0f * att_q[2]; 
		_8q1 = 8.0f * att_q[1];
		_8q2 = 8.0f * att_q[2];
		q0q0 = att_q[0] * att_q[0];
		q1q1 = att_q[1] * att_q[1];
		q2q2 = att_q[2] * att_q[2];
		q3q3 = att_q[3] * att_q[3];
		
		//加速度计更新四元数微分方程，梯度下降算法，计算误差的函数梯度                                                                                                                        
		s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;             
		s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * att_q[1] - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
		s2 = 4.0f * q0q0 * att_q[2] + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
		s3 = 4.0f * q1q1 * att_q[3] - _2q1 * ax + 4.0f * q2q2 * att_q[3] - _2q2 * ay;
		
		/*梯度归一化 */                         
		recipNorm=invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); 
		s0 *= recipNorm;
		s1 *= recipNorm;
		s2 *= recipNorm;
		s3 *= recipNorm;

		Tmep_Acce_Length=LIMIT(Acceleration_Length,0,20);//限幅 正常在10以内
		GYRO_rms=LIMIT(GYRO_Length,0,1000);//限幅 正常在500以内
		//BETADEF=Beta_Adjust[0]-Tmep_Acce_Length*0.005f+GYRO_rms*0.0001f;//动态步长 加速度模长抑制步长，角速度模长加速步长，抑制重力加速度的同时加速收敛
		BETADEF=Beta_Adjust[0];
		qDot1 -= BETADEF * s0;
		qDot2 -= BETADEF * s1;
		qDot3 -= BETADEF * s2;
		qDot4 -= BETADEF * s3;  
	}


	delta = (CNTLCYCLE * gx) * (CNTLCYCLE * gx) + (CNTLCYCLE * gy) * (CNTLCYCLE * gy) + (CNTLCYCLE * gz) * (CNTLCYCLE * gz);
	//二阶毕卡求解微分方程，补偿四元数微分方程引入的误差，积分四元数的导数得到姿态四元数
	att_q[0] = (1.0f - delta / 8.0f) * att_q[0] + qDot1 * CNTLCYCLE;                                       
	att_q[1] = (1.0f - delta / 8.0f) * att_q[1] + qDot2 * CNTLCYCLE;
	att_q[2] = (1.0f - delta / 8.0f) * att_q[2] + qDot3 * CNTLCYCLE;
	att_q[3] = (1.0f - delta / 8.0f) * att_q[3] + qDot4 * CNTLCYCLE;

	//四元数归一化
	recipNorm=invSqrt(att_q[0] * att_q[0] + att_q[1] * att_q[1] + att_q[2] * att_q[2] + att_q[3] * att_q[3]); 
	att_q[0] *= recipNorm;
	att_q[1] *= recipNorm;
	att_q[2] *= recipNorm;
	att_q[3] *= recipNorm;
	
	
	Att_Angle.pit = atan2(2.0f * att_q[2] * att_q[3] + 2.0f * att_q[0] * att_q[1], -2.0f * att_q[1] * att_q[1] - 2.0f * att_q[2]* att_q[2] + 1.0f) * RadtoDeg;
	Att_Angle.rol= asin(2.0f * att_q[0]* att_q[2]-2.0f * att_q[1] * att_q[3]) * RadtoDeg;								                   
//  Att_Angle.yaw=atan2(2.0f * att_q[1] * att_q[2] + 2.0f * att_q[0] * att_q[3], -2.0f * att_q[3] * att_q[3] - 2.0f * att_q[2] * att_q[2] + 1.0f) * RAD2DEG;

////////偏航角解算-陀螺仪磁力计协同融合////////////
	kalman_filter_yaw(Gyr_rad.Z*0.5729578f, mag_yaw, &kalman_yaw);		
	//Att_Angle.yaw = kalman_yaw - init_yaw;
	Att_Angle.yaw = 0.95f*Att_Angle.yaw + 0.05f*(kalman_yaw - init_yaw);
	if(Att_Angle.yaw>180) Att_Angle.yaw-=360;
	if(Att_Angle.yaw<-180) Att_Angle.yaw+=360;
	
	a_x_1=a_x;
	a_y_1=a_y;
	a_z=Acc_rad_filt.Z*cos(Att_Angle.pit*DegtoRad )*cos(Att_Angle.rol*DegtoRad )+Acc_rad_filt.Y*sin(Att_Angle.pit*DegtoRad )-Acc_rad_filt.X*cos(Att_Angle.pit*DegtoRad )*sin(Att_Angle.rol*DegtoRad )-G ;
	a_x=Acc_rad_filt.X*cos(Att_Angle.rol*DegtoRad )+Acc_rad_filt.Z*sin(Att_Angle.rol*DegtoRad ); 
	a_y=Acc_rad_filt.X*sin(Att_Angle.rol*DegtoRad )*sin(Att_Angle.pit*DegtoRad )+Acc_rad_filt.Y*cos(Att_Angle.pit*DegtoRad )-Acc_rad_filt.Z*sin(Att_Angle.pit*DegtoRad )*cos(Att_Angle.rol*DegtoRad );
	
		 
	v_x+=a_x;
	v_y+=a_y;
	v_z+=a_z; //cm/s
		 
		for(i=Quad_Num-1;i>0;i--) //保存四元数历史值    
	{
		Quad_Buf[i][0]=Quad_Buf[i-1][0];
		Quad_Buf[i][1]=Quad_Buf[i-1][1];
		Quad_Buf[i][2]=Quad_Buf[i-1][2];
		Quad_Buf[i][3]=Quad_Buf[i-1][3];
	}
		Quad_Buf[0][0]=att_q[0];     //最新四元数更新到末端备用
		Quad_Buf[0][1]=att_q[1];
		Quad_Buf[0][2]=att_q[2];
		Quad_Buf[0][3]=att_q[3];
}

/**************************实现函数*********************************************************************
函  数：static float invSqrt(float x) 
功　能: 快速计算 1/Sqrt(x) 	
参  数：要计算的值
返回值：结果
备  注：比普通Sqrt()函数要快四倍See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
*********************************************************************************************************/
static float invSqrt(float x) 
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}
